//
// Created by 86137 on 2024/3/19.
//

#include "send_calculate.h"
extern rc_device_t g_rc;
ctrl_msg_data my_msg={370,0,50,0,0};
float the;
float x_max;
float y_max;
float y=0;

void send_calculate(void){
    if(g_rc.rc_info.sw.sw_l==RC_SW_MID){
        my_msg.x=my_msg.x+(g_rc.rc_info.rocker.left_y/50)*7;
        my_msg.y=my_msg.y-(g_rc.rc_info.rocker.left_x/100)*7;
        my_msg.z=my_msg.z+(g_rc.rc_info.rocker.right_y/50)*7;
        my_msg.yaw=my_msg.yaw-(g_rc.rc_info.rocker.right_x/100)*7;
        the= atan2(my_msg.y,my_msg.x);
        y=sin(the);
        x_max=(ROUND)*cos(the);
        y_max=(ROUND)*y;
        if(pow(my_msg.x,2)+pow(my_msg.y,2)>pow(ROUND,2)){
            my_msg.x=x_max;
            my_msg.y=y_max;
        }
        if(my_msg.x<0) my_msg.x=0;
        if(my_msg.z<0) my_msg.z=0;
        if(my_msg.yaw>90) my_msg.yaw=90;
        if(my_msg.yaw<-90) my_msg.yaw=-90;
        if(my_msg.z>Z_MAX) my_msg.z=Z_MAX;
        if(g_rc.rc_info.sw.sw_r==1)  my_msg.is_cut_on=1;
        else my_msg.is_cut_on=0;
    }
}


